package edu.bmstu.rslab;

import java.awt.geom.Point2D;
import java.util.ArrayList;
import java.util.List;

/**
 * Created by Chikh (using IntelliJ IDEA).
 * Date: 05.03.2009
 * Time: 13:21:57
 */
public class Guidance {

    private final double PARAMETER_INCREMENT = 0.01;

    public Guidance(MobileRobot robot) {
        this.robot = robot;
    }

    public void sendControlSignal(Point2D position, Point2D directionPoint) {
        final double NUMBER_OF_POINTS = 3;
        double currentDirectionCos = (directionPoint.getX()-position.getX())/position.distance(directionPoint),
                currentDirectionSin = (directionPoint.getY()-position.getY())/position.distance(directionPoint);
        double minimum = Double.MAX_VALUE, alterminimum = Double.MAX_VALUE;
        ArrayList<Point2D> candidate = new ArrayList<Point2D>();
        for (int i = 0; i < NUMBER_OF_POINTS; i++) {
            for (Point2D point : result) {
               if ((i == 0) && (position.distance(point) != 0) && (position.distance(point) < minimum)) {
                   minimum = position.distance(point);
                   if (candidate.size() <= i) {
                       candidate.add(i, point);
                   }
                   else {
                       candidate.set(i, point);
                   }

                }
                else {
                   if ((i != 0) && (position.distance(point) < alterminimum) && (position.distance(point) > minimum)) {
                       alterminimum = position.distance(point);
                       if (candidate.size() <= i) {
                           candidate.add(i, point);
                       }
                       else {
                           candidate.set(i, point);
                       }
                   }
               }
            }
            if (i != 0) {
                minimum = alterminimum;
                alterminimum = Double.MAX_VALUE;
            }
        }

        if (candidate.size() != 0) {
            if (candidate.get(0) == result.get(result.size()-1)) {
                robot.stop();
            }
            else {
                double maximum = -1, differenceCos, turnCos = 1, turnSin = 0, candidateCos, candidateSin;
                for (Point2D point : candidate) {
                    candidateCos = (point.getX()-position.getX())/position.distance(point);
                    candidateSin = (point.getY()-position.getY())/position.distance(point);
                    differenceCos = currentDirectionCos*candidateCos + currentDirectionSin*candidateSin;
                    if (maximum < differenceCos) {
                        maximum = differenceCos;
                        turnCos = differenceCos;
                        turnSin = currentDirectionCos*candidateSin - currentDirectionSin*candidateCos;
                    }
                }
                robot.turnAnticlockwise(Math.atan2(turnSin, turnCos));
                robot.setVelocity(candidate.get(0).distance(candidate.get(1)));
            }
        }

    }
    public ArrayList<Point2D> calculateRoute(List<Point2D> checkPoints, Point2D initialDirection) {

        double[] t = new double[checkPoints.size()];
        t[0] = 0;
        for (int i = 1; i < checkPoints.size(); i++) {
            t[i] = checkPoints.get(i-1).distance(checkPoints.get(i));
        }

        double parameter = 0, tau,
                constantX = (checkPoints.get(1).getX() - checkPoints.get(0).getX()) / t[1],
                constantY = (checkPoints.get(1).getY() - checkPoints.get(0).getY()) / t[1];
        result = new ArrayList<Point2D>();

        if (checkPoints.size() == 2) {
            while (parameter < t[1]) {
                result.add(new Point2D.Double(constantX*parameter + checkPoints.get(0).getX(),
                        constantY*parameter + checkPoints.get(0).getY()));
                parameter = parameter + PARAMETER_INCREMENT;
            }
        }
        else {
            double[] alphaX = new double[checkPoints.size()];
            double[] alphaY = new double[checkPoints.size()];
            double[] betaX = new double[checkPoints.size()];
            double[] betaY = new double[checkPoints.size()];
            alphaX[1] = 0;
            betaX[1] = (initialDirection.getX() - checkPoints.get(0).getX())/
                    initialDirection.distance(checkPoints.get(0)); //Initial velocity (direction)
            alphaY[1] = 0;
            betaY[1] = (initialDirection.getY() - checkPoints.get(0).getY())/
                    initialDirection.distance(checkPoints.get(0)); //Initial velocity (direction)

            for (int j = 1; j < checkPoints.size() - 1; j++) {
                alphaX[j+1] = t[j] / (-2*(t[j] + t[j+1]) - alphaX[j]*t[j+1]);
                alphaY[j+1] = t[j] / (-2*(t[j] + t[j+1]) - alphaY[j]*t[j+1]);
                betaX[j+1] = (t[j+1]*betaX[j] - 3/ (t[j]*t[j+1]) * ( t[j]*t[j] * (checkPoints.get(j+1).getX() -
                        checkPoints.get(j).getX()) + t[j+1] *t[j+1] * (checkPoints.get(j).getX() -
                        checkPoints.get(j-1).getX())) ) / (-2*(t[j] + t[j+1]) - alphaX[j]*t[j+1]);
                betaY[j+1] = (t[j+1]*betaY[j] - 3/ (t[j]*t[j+1]) * ( t[j]*t[j] * (checkPoints.get(j+1).getY() -
                        checkPoints.get(j).getY()) + t[j+1] *t[j+1] * (checkPoints.get(j).getY() -
                        checkPoints.get(j-1).getY())) ) / (-2*(t[j] + t[j+1]) - alphaY[j]*t[j+1]);
            }

            double[] velocityX = new double[checkPoints.size()];
            double[] velocityY = new double[checkPoints.size()];
            velocityX[checkPoints.size() - 1] = (checkPoints.get(checkPoints.size()-1).getX() -
                    checkPoints.get(checkPoints.size()-2).getX()) / t[checkPoints.size()-1]; //Velocity (direction) in last point
            velocityY[checkPoints.size() - 1] = (checkPoints.get(checkPoints.size()-1).getY() -
                    checkPoints.get(checkPoints.size()-2).getY()) / t[checkPoints.size()-1]; //Velocity (direction) in last point

            for (int j = checkPoints.size() - 2; j >= 0; j--) {
                velocityX[j] = alphaX[j+1]*velocityX[j+1] + betaX[j+1];
                velocityY[j] = alphaY[j+1]*velocityY[j+1] + betaY[j+1];
            }

            for (int k = 0; k < checkPoints.size() - 1; k++) {
                parameter = 0;
                while (parameter < t[k+1]) {
                    tau = parameter / t[k+1];
                    result.add(new Point2D.Double((2*tau*tau*tau - 3*tau*tau + 1)*checkPoints.get(k).getX() +
                            (-2*tau*tau*tau + 3*tau*tau)*checkPoints.get(k+1).getX() + tau*(tau*tau - 2*tau +
                            1)*t[k+1]*velocityX[k] + tau*(tau*tau - tau)*t[k+1]*velocityX[k+1],
                            (2*tau*tau*tau - 3*tau*tau + 1)*checkPoints.get(k).getY() + (-2*tau*tau*tau +
                            3*tau*tau)*checkPoints.get(k+1).getY() + tau*(tau*tau - 2*tau + 1)*t[k+1]*velocityY[k] +
                            tau*(tau*tau - tau)*t[k+1]*velocityY[k+1]));
                    parameter = parameter + PARAMETER_INCREMENT;
                }
            }
        }
        return result;
    }

    private MobileRobot robot;
    private ArrayList<Point2D> result;
}